/* AK电机使用伺服模式速度位置环 扩展帧, YBT电机使用混合模式 标准帧*/

#ifndef MOTOR_CTRL_H
#define MOTOR_CTRL_H

#include <stdio.h>
#include <stdint.h>
#include <iostream>

#include "com_ctrl.h"

using namespace std;

#define MOTOR_NUM_MAX           (20)        //支持电机数
#define MOTOR_COMMON_FREQ       (0.01f)     //命令频率

/* 电机角度增大方向 */
typedef enum {
    DIR_CLOCKWISE = 1,        //顺时针
    DIR_COUNTER   = -1,       //逆时针
} Dir_Type;

typedef enum {
    MOTOR_TYPE_AK    = 0,
    MOTOR_TYPE_YBT   = 1,
    MOTOR_TYPE_VESC  = 2,
    MOTOR_TYPE_MAX,
} Motor_Type;

typedef struct {
    Motor_Type type;    //类型
    uint8_t id;         //id
    uint8_t group;      //组
    uint8_t chn;        //通道
} Motor_ID_Info;

typedef enum {
    MOTOR_MODE_ENTER = 0,       //进入
    MOTOR_MODE_EXIT,            //退出(复位)
    MOTOR_MODE_ORIGIN,          //原点
    MOTOR_MODE_W,               //速度
    MOTOR_MODE_POS_HOLD,        //位置固定
    MOTOR_MODE_POS_TRACK,       //位置追踪
    MOTOR_MODE_T,               //力矩
    MOTOR_MODE_MIX ,            //混合
} Motor_Mode;

/* 电机结构 */ 
class Motor {
public:
    Motor(Motor_ID_Info &info);
    virtual ~Motor() = 0;
    int32_t init();
    int32_t deinit();                   

    virtual int32_t set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len) = 0;     //设置帧
    virtual int32_t set_origin(uint8_t mode) = 0;         //设置零点                         
    virtual int32_t set_motor_pos(double pos) = 0;        //角度
    virtual int32_t set_motor_w(double w) = 0;            //角速度
    virtual int32_t set_motor_t(double t) = 0;            //前馈力矩
    virtual int32_t set_motor_acc(double acc) = 0;        //加速度
    virtual int32_t set_motor_kp(double kp) = 0;          //位置刚度
    virtual int32_t set_motor_kd(double kd) = 0;          //速度刚度
    virtual uint8_t get_motor_mode_map(Motor_Mode mode) = 0;   //模式映射
    virtual void set_cur_pos(uint8_t data[]) = 0;               //设置当前位置
    virtual void set_cur_w(uint8_t data[]) = 0;               //设置当前速度
    virtual void set_cur_tau(uint8_t data[]) = 0;               //设置当前扭矩
    inline double get_motor_acceptable_err() {return accept_err;}      //容许误差
    inline double get_motor_each_max() {return each_angle_max;}        //每次最大旋转角度
    inline double get_motor_cmd_freq() {return cmd_freq;}              //每次最大旋转角度
    inline double get_cur_pos() {return cur_pos;}
    inline double get_cur_w() {return cur_w;}
    inline double get_cur_tau() {return cur_tau;}
    inline Motor_Type get_motor_type() {return this->motor_info.type;}
    inline uint8_t get_motor_id() {return this->motor_info.id;}
    inline uint8_t get_motor_group() {return this->motor_info.group;}
    inline uint8_t get_motor_chn() {return this->motor_info.chn;}
    inline Dir_Type get_motor_dir() {return this->dir;}

    void set_motor_none();                                   //参数清零

protected:
    uint32_t flag_mix = 0; //混合控制标志
    Dir_Type dir = DIR_CLOCKWISE;
    double pos;
    double w;
    double t;
    double kp;
    double acc;
    double kd;

    double cur_pos      = 0.0f;        //当前角度 °
    double cur_w        = 0.0f;        //当前速度 °
    double cur_tau      = 0.0f;        //当前扭矩 N/M
    double accept_err   = 1.0f;        //容许误差(终值与期望的差)
    double each_angle_max = 10.0f;     //每次旋转最大角(保护作用)
    double cmd_freq     = MOTOR_COMMON_FREQ;        //命令间隔

private:
    Motor_ID_Info motor_info;
};

class Motor_AK : public Motor {
public:
#define AK_FREQ (0.002f)                  //AK最快1000Hz
#define AK_STATUS  (0x29)
#define AK_60_6_T_CONST (0.078f)        //ak60-6电机扭矩常数
#define AK_60_6_POLES   (14)            //极对数14
#define AK_60_6_V_RATIO (6.0f)          //减速比6：1
    Motor_AK(Motor_ID_Info &info);
    ~Motor_AK();

    int32_t set_motor_dataframe(uint8_t data[], Motor_Mode mode, double value[], uint8_t len) override;                  
    int32_t set_origin(uint8_t mode) override;                              
    int32_t set_motor_pos(double pos) override;  
    int32_t set_motor_w(double w) override;
    int32_t set_motor_t(double t) override; 
    int32_t set_motor_acc(double acc) override;
    int32_t set_motor_kp(double kp) override; 
    int32_t set_motor_kd(double kd) override; 
    uint8_t get_motor_mode_map(Motor_Mode mode) override;
    void set_cur_pos(uint8_t data[]) override;
    void set_cur_tau(uint8_t data[]) override;
    void set_cur_w(uint8_t data[]) override;
protected:

private:
};

class Motor_YBT : public Motor {
public:
#define YBT_FREQ (0.002f)                 //YBT最快500Hz
    Motor_YBT(Motor_ID_Info &info);
    ~Motor_YBT();

    int32_t set_motor_dataframe(uint8_t data[], Motor_Mode mode, double value[], uint8_t len) override;                    
    int32_t set_origin(uint8_t mode) override;                                       
    int32_t set_motor_pos(double pos) override;  
    int32_t set_motor_w(double w) override;
    int32_t set_motor_t(double t) override; 
    int32_t set_motor_acc(double acc) override;
    int32_t set_motor_kp(double kp) override; 
    int32_t set_motor_kd(double kd) override;
    uint8_t get_motor_mode_map(Motor_Mode mode) {return 0;} 
    void set_cur_pos(uint8_t data[]) override;
    void set_cur_w(uint8_t data[]) override;
    void set_cur_tau(uint8_t data[]) override;
protected:

private:
};


class Motor_VESC : public Motor {
public:
//TYI5008 电机
#define VESC_FREQ (0.002f)
#define VESC_STATUS_16  (0x10)         //含位置、电流、温度信息
#define VESC_STATUS_9  (0x09)         //含占空比、电流、转速信息

#define VESC_T_CONST    (1.0f)         // 没找到
#define VESC_POLES      (7)            // 极对数7 极数14
#define VESC_RATIO      (1.0f)         // 减速比1：1
    Motor_VESC(Motor_ID_Info &info);
    ~Motor_VESC();

    int32_t set_motor_dataframe(uint8_t data[], Motor_Mode mode, double value[], uint8_t len) override;                    
    int32_t set_origin(uint8_t mode) override;                                       
    int32_t set_motor_pos(double pos) override;  
    int32_t set_motor_w(double w) override;
    int32_t set_motor_t(double t) override; 
    int32_t set_motor_acc(double acc) override;
    int32_t set_motor_kp(double kp) override; 
    int32_t set_motor_kd(double kd) override;
    uint8_t get_motor_mode_map(Motor_Mode mode);
    void set_cur_pos(uint8_t data[]) override;
    void set_cur_w(uint8_t data[]) override;
    void set_cur_tau(uint8_t data[]) override;

    void set_start_offset(double data) {offset = data;}
    double get_start_offset() {return offset; }

protected:

private:
    int8_t cnt_round = 0;    //完整圈数
    double offset = 0.0f;     //起始角度偏移(上电时角度)
};

/* 电机控制体 */
class Motor_Ctrl {
    
public:
    Motor_Ctrl(Com_Ctrl *p_com_ctrl, uint8_t num,  Motor_ID_Info array[]);
    ~Motor_Ctrl();

    int motor_ctrl_init();
    int motor_ctrl_deinit();
    int motor_func(uint8_t num, uint8_t no[], Motor_Mode mode[], double* value[], uint8_t len[]);
    uint8_t get_motor_num(){return num;}
    Motor* get_motor_item(uint8_t num){return p_motor[num];}
protected:
    
private:
    Com_Ctrl *p_com_ctrl = nullptr;
    Motor *p_motor[MOTOR_NUM_MAX] = {nullptr};      //电机
    uint8_t num;

};

#endif